#! /usr/bin/env python
#-*- coding: utf-8 -*-
from naoqi import ALProxy
import sys
import time
from naoqi import *
import math
import almath
import time
import argparse
import re


def main(robotIP):
    PORT = 9559   

    try:
        memoryProxy = ALProxy("ALMemory", robotIP, PORT)
    except Exception, e:
        print "Could not create proxy to ALMemory"
        print "Error was: ", e

    try:
        sonarProxy = ALProxy("ALSonar", robotIP, PORT)
    except Exception, e:
        print "Could not create proxy to ALSonar"
        print "Error was: ", e


    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    motionProxy.wakeUp()       
    postureProxy.goToPosture("StandInit", 0.5)

    sonarProxy.subscribe("myApplication")
    while 1:
        print "Left  : "
        left=memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
        print left
        print "Right  : "
        Right=memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
        print  Right
        
        
        if(left>=0.5 and  Right>=0.5):
            print "continue\n\n\n"
            motionProxy.moveTo(0.2, 0, 0)      
        elif left<Right:
            print "turn right\n\n\n"
            motionProxy.moveTo(0, 0, -math.pi/3.0)
            motionProxy.moveTo(0.2, 0, 0)
            motionProxy.moveTo(0, 0, math.pi / 3.0)
        elif (left>Right):
            print "turn left\n\n\n" 
            motionProxy.moveTo(0, 0, math.pi/3.0)
            motionProxy.moveTo(0.2, 0, 0)
            motionProxy.moveTo(0, 0, -math.pi / 3.0)
        
        time.sleep(0.5)
            

    sonarProxy.unsubscribe("myApplication")

if __name__ == "__main__":
    robotIp = "192.168.201.69"

    if len(sys.argv) <= 1:
        print "Usage python sensors_sonar.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)